# Untitled - By: 64386 - 周四 8月 3 2023

import sensor, image, time, utime
import lcd, json
from Maix import GPIO
from fpioa_manager import fm
from machine import Timer, PWM, UART, Timer
import binascii
import math
#from board import board_info

lcd.init()
sensor.reset()
sensor.set_pixformat(sensor.GRAYSCALE) # 灰度更快(160x120 max on OpenMV-M7)
sensor.set_framesize(sensor.QQVGA)#160 * 120
sensor.skip_frames(time = 2000)

clock = time.clock()

tim1 = Timer(Timer.TIMER0, Timer.CHANNEL0, mode=Timer.MODE_PWM)
tim2 = Timer(Timer.TIMER0, Timer.CHANNEL1, mode=Timer.MODE_PWM)
S2 = PWM(tim1, freq=50, duty=0, pin=15)
S1 = PWM(tim2, freq=50, duty=0, pin=17)   #x
ratio = 6  #实际距离与像素距离之比
i_x = 0
i_y = 60
def Servo_x(servo,angle):
    #S2.duty((angle+90)/180*10+2.5)
    S1.duty((angle+90)/180*10+2.5)

def Servo_y(servo,angle):
    S2.duty((angle+90)/180*10+2.5)
   # S1.duty((angle+90)/180*10+2.5)
Servo_x(S1,0)
Servo_y(S2,-35)

flag = 1
fm.register(30,fm.fpioa.GPIOHS14)
fm.register(31,fm.fpioa.GPIOHS15)
fm.register(32,fm.fpioa.GPIOHS16)
fm.register(33,fm.fpioa.GPIOHS17)
fm.register(34,fm.fpioa.GPIOHS18)
fm.register(35,fm.fpioa.GPIOHS19)
#上拉按下为0 下拉按下为1
H1=GPIO(GPIO.GPIOHS15,GPIO.OUT)
H2=GPIO(GPIO.GPIOHS14,GPIO.OUT)
L1=GPIO(GPIO.GPIOHS16,GPIO.PULL_UP)
L2=GPIO(GPIO.GPIOHS17,GPIO.PULL_UP)
L3=GPIO(GPIO.GPIOHS18,GPIO.PULL_UP)
L4=GPIO(GPIO.GPIOHS19,GPIO.PULL_UP)
H1.value(1)
H2.value(1)
iii = 1
flag = 0
flag1 = 0
flag2 = 0
d1 = []
d2 = []
d3 = []
d4 = []
i_x = 0
i_y = 60
o_x = 0
o_y = 0
zb_x = []
zb_y = []
jiao_x = []
jiao_y = []
while(True):
    d = 0
    sensor.set_hmirror(1)
    clock.tick()
    img = sensor.snapshot()

    H1.value(0)
    if(L1.value()==0):  #确定原点
        o_x = i_x * 0.5
        o_y = i_y * 0.5
        print('已标记')
        utime.sleep_ms(200)
    if(L2.value()==0):  #y上
        Servo_y(S2,i_y*0.1)
        i_y = i_y - 2
        utime.sleep_ms(200)
    if(L3.value()==0):
        utime.sleep_ms(200)
    if(L4.value()==0):  #复位
        Servo_x(S1,o_x)
        Servo_y(S2,o_y)
    #        i_x = o_x
    #        i_y = o_y
    #        Servo_x(S1,i_x)
    #        Servo_y(S2,i_y)
        print('复位')
        utime.sleep_ms(200)
    H1.value(1)
    H2.value(0)
    if(L1.value()==0):  #x左
        Servo_x(S1,0.5*i_x)
        i_x = i_x + 1
        utime.sleep_ms(200)
    if(L2.value()==0):  #y下
        Servo_y(S2,i_y*0.5)
        i_y = i_y + 2
        utime.sleep_ms(200)
    if(L3.value()==0):  #x右
        Servo_x(S1,0.5*i_x)
        i_x = i_x - 1
        utime.sleep_ms(200)
    if(L4.value()==0):
        utime.sleep_ms(200)
    H2.value(1)

    while flag < 1:
        for r in img.find_rects(threshold = 30000):
            i = r.corners()
            print(i)
            for p in r.corners(): img.draw_circle(p[0], p[1], 5, color = (0, 255, 0))  #在四个角上画圆

            d1.append(i[3])
            d2.append(i[2])
            d3.append(i[1])
            d4.append(i[0])
            flag = flag + 1
            #print(flag)
            #print(d1)
            time.sleep(3)

            zb_x.append(d1[0][0])
            zb_x.append(d2[0][0])
            zb_x.append(d3[0][0])
            zb_x.append(d4[0][0])
            zb_y.append(d1[0][1])
            zb_y.append(d2[0][1])
            zb_y.append(d3[0][1])
            zb_y.append(d4[0][1])
            #print(zb_x)
            #print(zb_y)
            #lcd.display(img)
    while flag1 < 4 :
        #lcd.display(img)
        x = (zb_x[flag1])*ratio
        y = (zb_y[flag1])*ratio
        #print(x)
        b = 1000
        a = math.sqrt(b*b + (480-x)*(480-x))
        c = math.sqrt(b*b + (360-y)*(360-y))
        if x <= 480:          #舵机位于底部中央（160，240） 或 （80,120）
            jiaoX = math.degrees(math.atan((480-x)/c))

        else:
            jiaoX = -math.degrees((math.atan((x-480)/c)))


        if y <= 360:
            jiaoY = math.degrees(math.atan((360-y)/a))
        else:
            jiaoY = math.degrees(math.atan((y-360)/a))
        #print(jiaoA1)
        jiao_x.append(jiaoX)
        jiao_y.append(jiaoY)
        flag1 = flag1 + 1
    Servo_x(S1,jiao_x[0])

    Servo_y(S2,jiao_y[0])
    while flag2 < 4 :
        while iii < 400:
            flag3 = flag2 + 1
            if flag3 == 4:
                flag3 = 0
            bujin_x = jiao_x[flag2] + (((jiao_x[flag3])-(jiao_x[flag2]))/400)*iii
            bujin_y = jiao_y[flag2] + (((jiao_y[flag3])-(jiao_y[flag2]))/400)*iii - 45
            Servo_x(S1,-bujin_x)
            Servo_y(S2,bujin_y)
            print(bujin_x)
            print(bujin_y)
            iii = iii + 1
            utime.sleep_ms(2)
            #lcd.display(img)
        utime.sleep_ms(200)
        #x1 = jiao_x[flag2]
        #y1 = jiao_y[flag2] - 45

        flag2 = flag2 + 1
        #Servo_x(S1,x1)
        #Servo_y(S2,y1)
        #utime.sleep_ms(1000)
        #time.sleep(1)
            #Servo(S2,jiaoB)
        iii = 1
        #flag2 = flag2 + 1



